#define P_GAIN 0.15
#define I_GAIN 0.00001
#define D_GAIN 0.01

#define F_CPU 1000000
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdio.h>

int set_rpm = 0,previous_error = 0;
int P_error, feedback_rpm, output;
int D_error = 0, I_error = 0;

short c = 0;
unsigned int pulses;
int ADCval;
char outs[30];

//********************************//
void usart_init (void)
{
	//synchronous usart, transmit 8-bit data
	UCSR0C = ((1<<UCSZ01)|(1<<UCSZ00)|(1<<UMSEL00));
	//9600 Baud Rate from 8MHz clock (0x01A0)
	//9600 Baud Rate from 1MHz clock (0x0033)
	UBRR0H = 0x00;
	UBRR0L = 0x33;
	UCSR0B = (1<<TXEN0);	//enable transmitter
}
void USART_tx_string (char *data)
{
	while((*data!='\0')){
		while(!(UCSR0A&(1<<UDRE0)));			//wait until transmit register is empty
		UDR0 = *data;
		data++;
	}
}
void init_interrupts(){
	sei();
}
void init_io(){
	// Normal Mode
	// 1024 prescalar
	// Input capture edge select on ICP1
	// 1uS * 1024 = ~1mS per count
	TCNT1L = 0;
	TCNT1H = 0;
	TIMSK1 = (1 << ICIE1);
	TCCR1B = (1 << CS12) | (1 << CS10) | (1<<ICES1);
	
	// Timer0 CTC mode Frequency = F_CPU/(2*N*(1+OCR0))
	// Toggle OCR0A on compare match
	TCNT0 = 0x00;
	OCR0A = 243;
	// Prescalar 1024
	TCCR0A |= (1 << WGM01) | (1 << WGM00) | (1 << COM0A1);
	TCCR0B |= (1 << CS00) | (1 << CS01);
}
ISR(TIMER1_CAPT_vect){
	pulses++;
	if(pulses==16){
		feedback_rpm = ICR1;					//feedback_rpm = period of one rotation in ms
		TCNT1 = 0;								//Zero the count register
		feedback_rpm = 60000/feedback_rpm;		//feedback_rpm = rotations/minute for inner gears
		feedback_rpm = feedback_rpm/19;			//feedback_rpm = rotations/minute for outer rotor
		pulses = 0;
	}
}
//function to map x to out
long map(long x, long in_min, long in_max, long out_min, long out_max)
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int readADC(void){
	ADMUX = (ADMUX & 0xF0)|(0 & 0x0F);	//ADC0
	ADCSRA |= 1<<ADSC;					//Convert
	while(ADCSRA & (1<<ADSC));			//Wait until finished
	ADCval = (ADC>>6);					//Shift right 6 to use converted value
	ADCval = (int)map((long)ADCval, 0,1023,0,500);
	return ADCval;
}
int main(){
	ADMUX = (1<<REFS0)|(1<<ADLAR);	//5V reference and left justified
	ADCSRA = (1<<ADEN);				//Enable ADC
	ADCSRB = 0x00;					//Free running mode
	
	DDRD = 0xf0;
	DDRB = 0xfe;
	//set_rpm = 100;
	
	init_interrupts();
	init_io();
	usart_init();
	
	previous_error = set_rpm - feedback_rpm;
	//Start infinite loop
	while(1){
		_delay_ms(100);
		set_rpm = readADC();

		P_error = set_rpm - feedback_rpm;
		
		I_error += (P_error);
		D_error = (P_error - previous_error);
		
		output = (P_GAIN * P_error) + (I_GAIN * I_error) + (D_GAIN * D_error);
		
		previous_error = P_error;
		
		snprintf(outs,sizeof(outs),"{SET,T,%3d}\r\n",set_rpm+'0'-48);
 		USART_tx_string(outs);
 		snprintf(outs,sizeof(outs),"{RPM,T,%3d}\r\n",feedback_rpm+'0'-48);
 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"Set_rpm: %3d RPM, ",set_rpm+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"feedback_rpm: %3d RPM, ",feedback_rpm+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"Error: %3d , ",P_error+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"I_error: %3d , ",I_error+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"D_error: %3d , ",D_error+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"output: %3d , ",output+'0'-48);
// 		USART_tx_string(outs);
// 		snprintf(outs,sizeof(outs),"previous: %3d \r\n",output+'0'-48);
// 		USART_tx_string(outs);
		
		if(OCR0A + output > 255){
			OCR0A = 255;
		}
		else if(OCR0A + output < 0){
			OCR0A = 0;
		}
		else{
			OCR0A += output;
		}
	} //End of while
	return 0;
} //End of main